/*
 * ball_catcher.cc
 *
 * Implementation of the strategy to catch a ball
 */

#include "xtask.h"
#include "xqueue.h"
#include "log.h"
#include "ball_catcher.h"
#include "robo_control.h"
#include "modules/Vision/VisionManager.h"
#include "modules/Util/ConfigFile.h"
#include <cmath>

using namespace kthrobot;
using namespace std;

namespace Group5 {

    BallCatcher::BallCatcher(Robo_Control* motor1, Robo_Control* motor2)
        : XTask("BallCatcher"),
        motor1_(motor1), motor2_(motor2) {
            SetPeriodHz(hz_);
        }


    BallCatcher::~BallCatcher() {
        Join();
    }


    void BallCatcher::TaskCode() {

        VisionManager::getInstance().Up();
        double speedL, speedR;

        while(!MustStop()) {
            DetectionResults ball_results = VisionManager::getInstance().getBallResult();
            
            // If the ball has been lost, quit
            if (!ball_results.detected) {
                Log::Write("BallCatcher: No ball detected");
                speedL = 0; //BASE_SPEED;
                speedR = 0; //BASE_SPEED;
            }
            else {
                Log::Write("BallCatcher: Ball detected");
                Log::Write("BallCatcher: x = %i, y = %i, w = %i, h = %i, s = %i",
                        ball_results.x, ball_results.y, ball_results.w, ball_results.h, ball_results.size);

                // If we are too close from the ball back off
                if (ball_results.w > MAXWIDTH && 0) {
                    Log::Write("BallCatcher: Ball too close");
                    speedL = - BASE_SPEED;
                    speedR = - BASE_SPEED;
                }

                // Otherwise go forward aiming for the ball
                else {
                    Log::Write("BallCatcher: Catching the ball!");
                    // Coordinates of the center of the detected ball
                    uint8_t center_x = ball_results.x; //+ball_results.w/2;
                    Log::Write("BallCatcher: computed center = %d", center_x);
                    //uint8_t center_y = ball_results.y+ball_results.w/2;
                    // Horizontal distance to the center, between -1 (left) and +1 (right)
                    int IMG_X = Constants::get().IMG_X;
                    double diff = (2*center_x - IMG_X) / (double) IMG_X;
                    Log::Write("BallCatcher: diff = %f", diff);

                    speedL = (1+0.1*diff)*BASE_SPEED;
                    speedR = (1-0.1*diff)*BASE_SPEED;
                }
            }

            motor1_->SetTargetSpeed(speedL);
            motor2_->SetTargetSpeed(-speedR);

            WaitPeriod();
        }
        VisionManager::getInstance().Down();
    };

};
